/*
 * Copyright 2010 The MIauto Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include <avr/io.h>
#include <avr/eeprom.h>
#include <stdint.h>

#include "servo.h"


#define VERIFY_BYTE 0b01010101
#define DEFAULT_SERVO(x,y) {3,661, 1045, 280, 661, &x ,(void*)&teServos[y]}

uint8_t EEMEM ue8VerifyByte = VERIFY_BYTE;

t_Servo EEMEM teServos[SERVO_COUNT];

volatile t_Servo servos[SERVO_COUNT] =
{
DEFAULT_SERVO(OCR1A, DIRECTION_SERVO),
DEFAULT_SERVO(OCR1B, THROTLE_SERVO) };

uint8_t u8VerifyByte = 0x00;

static void init_servo_eeprom(void);

void PWM_Init(void)
{

	/* Phase and Frequency correct PWM-mode, TOP = ICR1 */
	TCCR1A |= (0 << WGM11) | (0 << WGM10);

	/* Clear OC1A on compare match when upcounting, set OC1a on compare match when downcounting */
	TCCR1A |= (1 << COM1A1) | (0 << COM1A0);

	/* clear OC1B on compare match when upcounting, set OC1B on compare match when downcounting */
	TCCR1A |= (1 << COM1B1) | (0 << COM1B0);

	/* Waveform Generation Mode */
	TCCR1B |= (1 << WGM13) | (0 << WGM12);

	/* clock select */
	TCCR1B |= (0 << CS12) | (1 << CS11) | (0 << CS10); /* clk/8 (from prescaler) */

	/* set PWM frequency */
	ICR1 = 10000; /* 20ms */

	/* PD4/OC1B and PD5/OC1A as output */
	DDRD |= (1 << PD4) | (1 << PD5);

	eeprom_read_block((void*) &u8VerifyByte, (const void*) &ue8VerifyByte, 1);

	if (u8VerifyByte != VERIFY_BYTE)
		init_servo_eeprom();

	/* load servo settings */
	int i = 0;
	for (; i < SERVO_COUNT; i++)
		eeprom_read_block((void*) &servos[i], (const void*) servos[i].pMemAdd,
				T_SERVO_STRUCT_SIZE);

	stopServos();
}

static void init_servo_eeprom(void)
{
	u8VerifyByte = VERIFY_BYTE;

	eeprom_write_block((const void*) &u8VerifyByte, (void*) &ue8VerifyByte, 1);

	int i = 0;
	for (; i < SERVO_COUNT; i++)
		saveServoSettings(&servos[i]);

}

void setServoSettings(volatile t_Servo *tServo, uint8_t u8Mul,
		uint16_t u16DefDir, uint16_t u16MaxDir, uint16_t u16MinDir,
		uint16_t u16Dir, volatile uint16_t *pu8ocr)
{

	tServo->u8Multiplyer = u8Mul;
	tServo->u16DefaultDirection = u16DefDir;
	tServo->u16MaxDirection = u16MaxDir;
	tServo->u16MinDirection = u16MinDir;
	tServo->u16Direction = u16Dir;
	tServo->ocr = pu8ocr;

}

void saveServoSettings(volatile t_Servo *tServo)
{
	eeprom_write_block((const void*) tServo, (void*) tServo->pMemAdd,
			T_SERVO_STRUCT_SIZE);

}

void stopServos()
{
	int i = 0;
	for (; i < SERVO_COUNT; i++)
		stopServo(&servos[i]);
}

void stopServo(volatile t_Servo *tServo)
{
	setDirect(tServo, tServo->u16DefaultDirection);
}

void setDirect(volatile t_Servo *tServo, uint8_t u8Dir)
{
	if (u8Dir > 128)
	{
		tServo->u16Direction = tServo->u16DefaultDirection + ((u8Dir - 127)
				* tServo->u8Multiplyer);
		if (tServo->u16Direction > tServo->u16MaxDirection)
			tServo->u16MaxDirection = tServo->u16MaxDirection;
		*tServo->ocr = tServo->u16Direction;
	}
	else if (u8Dir < 128)
	{
		tServo->u16Direction = tServo->u16DefaultDirection - ((126 - u8Dir)
				* tServo->u8Multiplyer);
		if (tServo->u16Direction < tServo->u16MinDirection)
			tServo->u16Direction = tServo->u16MinDirection;
		*tServo->ocr = tServo->u16Direction;
	}
	else
	{
		*tServo->ocr = tServo->u16DefaultDirection;
	}
}

